// 相机标定数据读取程序
// 功能：通过/camera/color/camera_info读取松灵小车的color相机的标定数据

#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <iostream>
#include <fstream>
#include <yaml-cpp/yaml.h>

class CameraInfoReader {
private:
    ros::NodeHandle nh_;
    ros::Subscriber camera_info_sub_;
    std::string output_file_;
    bool data_received_;

public:
    CameraInfoReader() : data_received_(false) {
        // 订阅相机信息话题
        camera_info_sub_ = nh_.subscribe("/camera/color/camera_info", 10, 
                                        &CameraInfoReader::cameraInfoCallback, this);
        
        // 设置输出文件路径
        output_file_ = "camera_info_from_topic.yaml";
        
        ROS_INFO("Camera Info Reader initialized");
        ROS_INFO("Waiting for camera info data...");
    }

    void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) {
        if (data_received_) return; // 只处理一次
        
        ROS_INFO("Received camera info data");
        
        // 创建YAML文档
        YAML::Node camera_info;
        
        // 相机内参矩阵
        camera_info["camera_matrix"] = YAML::Node(YAML::NodeType::Map);
        camera_info["camera_matrix"]["rows"] = 3;
        camera_info["camera_matrix"]["cols"] = 3;
        camera_info["camera_matrix"]["data"] = std::vector<double>(msg->K.begin(), msg->K.end());
        
        // 畸变系数
        camera_info["distortion_coefficients"] = YAML::Node(YAML::NodeType::Map);
        camera_info["distortion_coefficients"]["rows"] = 1;
        camera_info["distortion_coefficients"]["cols"] = msg->D.size();
        camera_info["distortion_coefficients"]["data"] = std::vector<double>(msg->D.begin(), msg->D.end());
        
        // 投影矩阵
        camera_info["projection_matrix"] = YAML::Node(YAML::NodeType::Map);
        camera_info["projection_matrix"]["rows"] = 3;
        camera_info["projection_matrix"]["cols"] = 4;
        camera_info["projection_matrix"]["data"] = std::vector<double>(msg->P.begin(), msg->P.end());
        
        // 图像尺寸
        camera_info["image_width"] = msg->width;
        camera_info["image_height"] = msg->height;
        
        // 相机名称
        camera_info["camera_name"] = msg->header.frame_id;
        
        // 保存到文件
        std::ofstream fout(output_file_);
        fout << camera_info;
        fout.close();
        
        ROS_INFO("Camera info saved to: %s", output_file_.c_str());
        
        // 打印关键信息
        ROS_INFO("=== Camera Calibration Data ===");
        ROS_INFO("Image size: %dx%d", msg->width, msg->height);
        ROS_INFO("Camera matrix:");
        for (int i = 0; i < 3; i++) {
            ROS_INFO("  [%.6f, %.6f, %.6f]", msg->K[i*3], msg->K[i*3+1], msg->K[i*3+2]);
        }
        ROS_INFO("Distortion coefficients:");
        for (size_t i = 0; i < msg->D.size(); i++) {
            ROS_INFO("  %.6f", msg->D[i]);
        }
        
        data_received_ = true;
        ROS_INFO("Camera info reading completed. Press Ctrl+C to exit.");
    }
    
    bool isDataReceived() const {
        return data_received_;
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "camera_info_reader");
    
    CameraInfoReader reader;
    
    ros::Rate rate(10);
    while (ros::ok() && !reader.isDataReceived()) {
        ros::spinOnce();
        rate.sleep();
    }
    
    // 保持节点运行，直到用户中断
    while (ros::ok()) {
        ros::spinOnce();
        rate.sleep();
    }
    
    return 0;
}
